
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors_multicopter.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_motors_multicopter.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
void MotorsMC_set_yaw_headroom(MotorsMC_HandleTypeDef *pMotors, int16_t pwm)
{
    pMotors->_yaw_headroom = pwm;
}

// passes throttle directly to all motors for ESC calibration.
//   throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
void MotorsMC_set_throttle_passthrough_for_esc_calibration(MotorsMC_HandleTypeDef *pMotors, float throttle_input)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    if (Motors_get_armed(motors)) {
        uint16_t pwm_out = MotorsMC_get_pwm_output_min(pMotors) + math_constrain_float(throttle_input, 0.0f, 1.0f) * (MotorsMC_get_pwm_output_max(pMotors) - MotorsMC_get_pwm_output_min(pMotors));
        // send the pilot's input directly to each enabled motor
        for (uint16_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
            if (pMotors->motor_enabled[i]) {
                //TODO: 输出PWM
                Motors_rc_write(motors, i, pwm_out);
            }
        }

        // send pwm output to channels used by bicopter
        srv_channels_set_output_pwm(k_throttleRight, pwm_out);
        srv_channels_set_output_pwm(k_throttleLeft, pwm_out);
    }
}

// get minimum pwm value that can be output to motors
int16_t MotorsMC_get_pwm_output_min(MotorsMC_HandleTypeDef *pMotors)
{
    // return _pwm_min if both PWM_MIN and PWM_MAX parameters are defined and valid
    if ((pMotors->_pwm_min > 0) && (pMotors->_pwm_max > 0) && (pMotors->_pwm_max > pMotors->_pwm_min)) {
        return pMotors->_pwm_min;
    }
    return pMotors->_throttle_radio_min;
}

// get maximum pwm value that can be output to motors
int16_t MotorsMC_get_pwm_output_max(MotorsMC_HandleTypeDef *pMotors)
{
    // return _pwm_max if both PWM_MIN and PWM_MAX parameters are defined and valid
    if ((pMotors->_pwm_min > 0) && (pMotors->_pwm_max > 0) && (pMotors->_pwm_max > pMotors->_pwm_min)) {
        return pMotors->_pwm_max;
    }
    return pMotors->_throttle_radio_max;
}

// get_lift_max - get maximum lift ratio - for logging purposes only
float MotorsMC_get_lift_max(MotorsMC_HandleTypeDef *pMotors)
{
    return pMotors->_lift_max;
}

// get_batt_voltage_filt - get battery voltage ratio - for logging purposes only
float MotorsMC_get_batt_voltage_filt(MotorsMC_HandleTypeDef *pMotors)
{
    return lpf_get_output(&(pMotors->_batt_voltage_filt));
}

// get throttle limit imposed by battery current limiting.  This is a number from 0 ~ 1 where 0 means hover throttle, 1 means full throttle (i.e. not limited)
float MotorsMC_get_throttle_limit(MotorsMC_HandleTypeDef *pMotors)
{
    return pMotors->_throttle_limit;
}

// returns maximum thrust in the range 0 to 1
float MotorsMC_get_throttle_thrust_max(MotorsMC_HandleTypeDef *pMotors)
{
    return pMotors->_throttle_thrust_max;
}

// return true if spool up is complete
bool MotorsMC_spool_up_complete(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;
    return motors->_spool_state == MOTOR_THROTTLE_UNLIMITED;
}

// parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool MotorsMC_check_mot_pwm_params(MotorsMC_HandleTypeDef *pMotors)
{
    // both must be zero or both non-zero:
    if (pMotors->_pwm_min == 0 && pMotors->_pwm_max != 0) {
        return false;
    }
    if (pMotors->_pwm_min != 0 && pMotors->_pwm_max == 0) {
        return false;
    }
    // sanity says that minimum should be less than maximum:
    if (pMotors->_pwm_min != 0 && pMotors->_pwm_min >= pMotors->_pwm_max) {
        return false;
    }
    // negative values are out-of-range:
    if (pMotors->_pwm_min < 0 || pMotors->_pwm_max < 0) {
        return false;
    }

    return true;
}

// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float MotorsMC_apply_thrust_curve_and_volt_scaling(MotorsMC_HandleTypeDef *pMotors, float thrust)
{
    float throttle_ratio = thrust;
    // apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
    float thrust_curve_expo = math_constrain_float(pMotors->_thrust_curve_expo, -1.0f, 1.0f);
    if (fabsf(thrust_curve_expo) < 0.001f) {
        // zero expo means linear, avoid floating point exception for small values
        return thrust;
    }
    if (!math_flt_zero(lpf_get_output(&(pMotors->_batt_voltage_filt)))) {
        throttle_ratio = ((thrust_curve_expo - 1.0f) + math_sqrtf((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * pMotors->_lift_max * thrust)) / (2.0f * thrust_curve_expo * lpf_get_output(&(pMotors->_batt_voltage_filt)));
    } else {
        throttle_ratio = ((thrust_curve_expo - 1.0f) + math_sqrtf((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * pMotors->_lift_max * thrust)) / (2.0f * thrust_curve_expo);
    }

    return math_constrain_float(throttle_ratio, 0.0f, 1.0f);
}

// converts desired thrust to linearized actuator output in a range of 0~1
float MotorsMC_thrust_to_actuator(MotorsMC_HandleTypeDef *pMotors, float thrust_in)
{
    thrust_in = math_constrain_float(thrust_in, 0.0f, 1.0f);
    return pMotors->_spin_min + (pMotors->_spin_max - pMotors->_spin_min) * MotorsMC_apply_thrust_curve_and_volt_scaling(pMotors, thrust_in);
}

// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
float MotorsMC_get_current_limit_max_throttle(MotorsMC_HandleTypeDef *pMotors)
{
    return 1.0f;
}

// update the throttle input filter
void MotorsMC_update_throttle_filter(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    if (Motors_get_armed(motors)) {
        lpf_apply2(&(motors->_throttle_filter),motors->_throttle_in, 1.0f / motors->_loop_rate);
        // constrain filtered throttle
        if (lpf_get_output(&(motors->_throttle_filter)) < 0.0f) {
            lpf_reset(&(motors->_throttle_filter),0.0f);
        }
        if (lpf_get_output(&(motors->_throttle_filter)) > 1.0f) {
            lpf_reset(&(motors->_throttle_filter),1.0f);
        }
    } else {
        lpf_reset(&(motors->_throttle_filter),0.0f);
    }
}

// update_lift_max_from_batt_voltage - used for voltage compensation
void MotorsMC_update_lift_max_from_batt_voltage(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    // sanity check battery_voltage_min is not too small
    // if disabled or misconfigured exit immediately
    float _batt_voltage_resting_estimate = 0.0f;
    if ((pMotors->_batt_voltage_max <= 0.0f) || (pMotors->_batt_voltage_min >= pMotors->_batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f * pMotors->_batt_voltage_min)) {
        lpf_reset(&(pMotors->_batt_voltage_filt),1.0f);
        pMotors->_lift_max = 1.0f;
        return;
    }

    pMotors->_batt_voltage_min = MAX(pMotors->_batt_voltage_min, pMotors->_batt_voltage_max * 0.6f);

    // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
    _batt_voltage_resting_estimate = math_constrain_float(_batt_voltage_resting_estimate, pMotors->_batt_voltage_min, pMotors->_batt_voltage_max);

    // filter at 0.5 Hz
    float batt_voltage_filt = lpf_apply2(&(pMotors->_batt_voltage_filt),_batt_voltage_resting_estimate / pMotors->_batt_voltage_max, 1.0f / motors->_loop_rate);

    // calculate lift max
    float thrust_curve_expo = math_constrain_float(pMotors->_thrust_curve_expo, -1.0f, 1.0f);
    pMotors->_lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;
}

// return gain scheduling gain based on voltage and air density
float MotorsMC_get_compensation_gain(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    // avoid divide by zero
    if (pMotors->_lift_max <= 0.0f) {
        return 1.0f;
    }

    float ret = 1.0f / pMotors->_lift_max;

#if GP_MOTORS_DENSITY_COMP == 1
    // air density ratio is increasing in density / decreasing in altitude
    if (motors->_air_density_ratio > 0.3f && motors->_air_density_ratio < 1.5f) {
        ret *= 1.0f / math_constrain_float(motors->_air_density_ratio, 0.5f, 1.25f);
    }
#endif
    return ret;
}

// convert actuator output (0~1) range to pwm range
int16_t MotorsMC_output_to_pwm(MotorsMC_HandleTypeDef *pMotors, float _actuator_output)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    int16_t pwm_output;
    if (motors->_spool_state == MOTOR_SHUT_DOWN) {
        // in shutdown mode, use PWM 0 or minimum PWM
        if (pMotors->_disarm_disable_pwm && !Motors_get_armed(motors)) {
            pwm_output = 0;
        } else {
            pwm_output = MotorsMC_get_pwm_output_min(pMotors);
        }
    } else {
        // in all other spool modes, covert to desired PWM
        pwm_output = MotorsMC_get_pwm_output_min(pMotors) + (MotorsMC_get_pwm_output_max(pMotors) - MotorsMC_get_pwm_output_min(pMotors)) * _actuator_output;
    }

    return pwm_output;

}

// adds slew rate limiting to actuator output if MOT_SLEW_TIME > 0 and not shutdown
void MotorsMC_set_actuator_with_slew(MotorsMC_HandleTypeDef *pMotors, float* actuator_output, float input)
{
    /*
    If MOT_SLEW_UP_TIME is 0 (default), no slew limit is applied to increasing output.
    If MOT_SLEW_DN_TIME is 0 (default), no slew limit is applied to decreasing output.
    MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME are constrained to 0.0~0.5 for sanity.
    If spool mode is shutdown, no slew limit is applied to allow immediate disarming of motors.
    */

    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    // Output limits with no slew time applied
    float output_slew_limit_up = 1.0f;
    float output_slew_limit_dn = 0.0f;

    // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
    if (math_flt_positive(pMotors->_slew_up_time)) {
        float output_delta_up_max = 1.0f / (math_constrain_float(pMotors->_slew_up_time, 0.0f, 0.5f) * motors->_loop_rate);
        output_slew_limit_up = math_constrain_float(*actuator_output + output_delta_up_max, 0.0f, 1.0f);
    }

    // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
    if (math_flt_positive(pMotors->_slew_dn_time)) {
        float output_delta_dn_max = 1.0f / (math_constrain_float(pMotors->_slew_dn_time, 0.0f, 0.5f) * motors->_loop_rate);
        output_slew_limit_dn = math_constrain_float(*actuator_output - output_delta_dn_max, 0.0f, 1.0f);
    }

    // Constrain change in output to within the above limits
    *actuator_output = math_constrain_float(input, output_slew_limit_dn, output_slew_limit_up);
}

// gradually increase actuator output to ground idle
float MotorsMC_actuator_spin_up_to_ground_idle(MotorsMC_HandleTypeDef *pMotors)
{
    return math_constrain_float(pMotors->_spin_up_ratio, 0.0f, 1.0f) * pMotors->_spin_min;
}

// output booster throttle, if any
void MotorsMC_output_boost_throttle(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    if (pMotors->_boost_scale > 0.0f) {
        float throttle = math_constrain_float(Motors_get_throttle(motors) * pMotors->_boost_scale, 0.0f, 1.0f);
        srv_channels_set_output_scaled(k_boost_throttle, throttle * 1000);
    } else {
        srv_channels_set_output_scaled(k_boost_throttle, 0);
    }
}

// update_throttle_range - update throttle endpoints
void MotorsMC_update_throttle_range(MotorsMC_HandleTypeDef *pMotors)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotors;

    // if all outputs are digital adjust the range. We also do this for type PWM_RANGE, as those use the
    // scaled output, which is then mapped to PWM via the SRV_Channel library
    if (srv_channels_have_digital_outputs(MotorsMC_get_motor_mask()) || (motors->_pwm_type == PWM_TYPE_PWM_RANGE)) {
        pMotors->_pwm_min = 1000;
        pMotors->_pwm_max = 2000;
    }

    srv_hal_set_esc_scaling(MotorsMC_get_pwm_output_min(pMotors), MotorsMC_get_pwm_output_max(pMotors));
}

/*------------------------------------test------------------------------------*/


